#ifndef _KNOWLEDGE_BASE_H_
#define _KNOWLEDGE_BASE_H_

#include "/home/sabertooth/ROS/sabertooth/include/path/path.h"
#include <semaphore.h> 
#include <stdlib.h>
#include <iostream>
using std::cerr;
using std::endl;

class KnowledgeBase {

private:
	int** worldMap;
	Path* aStarPath;

	// Semaphores
	sem_t mapSem, lidarSem, pathSem, gaitSem;

	// Enablers
	bool scanLidar, planPath, planGait;

public:
	KnowledgeBase() { 
		aStarPath = NULL; 

		scanLidar = false;
		planPath = false;
		planGait = false;

		// Initialize semaphors
		sem_init(&mapSem, 0, 1);
		sem_init(&lidarSem, 0, 1);
		sem_init(&pathSem, 0, 1);
	        sem_init(&gaitSem, 0, 1);
	}

	virtual ~KnowledgeBase() {
		sem_destroy(&mapSem);
		sem_destroy(&lidarSem);
		sem_destroy(&pathSem);
        	sem_destroy(&gaitSem);
	}

	// Map data
	void setWorldMap(int** map) {
		sem_wait(&mapSem);
		worldMap = map;
		sem_post(&mapSem);
	}

	int** getWorldMap() {
		return worldMap;	 		
	}

	// Path data
	Path* getPath() {
		return aStarPath;
	}

	void setPath(Path* path) {
		sem_wait(&pathSem);
		aStarPath = path;
                sem_post(&pathSem);
	}

	// Enablers

	void setScanLidar(bool b) {
		sem_wait(&lidarSem);
		scanLidar = b;
                sem_post(&lidarSem);
	}

	bool getScanLidar() {
		return scanLidar;
	}

	void setPlanPath(bool b) {
		sem_wait(&pathSem);
		planPath = b;
                sem_post(&pathSem);
	}

	bool getPlanPath() {
		return planPath;
	}

	void setPlanGait(bool b) {
		sem_wait(&gaitSem);
		planGait = b;
                sem_post(&gaitSem);
	}

	bool getPlanGait() {
		return planGait;
	}
};

#endif
